/* @FileName:peripheral.h
 * @Author: haibo.yang
 * @Date: 2022-09-1  08:55:14
 * @Last Modified by: haibo.yang
 * @Last Modified time: 2022-09-1 11:17:39
 */
#pragma once

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float64.hpp>
#include <agv_actions/action/peripheral.hpp>
#include <agv_msgs/msg/action.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/periph_teleop.hpp>
#include <agv_msgs/msg/agv_mode.hpp>
#include <agv_msgs/msg/load.hpp>
#include <agv_msgs/msg/drivers_info.hpp>
#include <agv_msgs/msg/set_stop_bits.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include <agv_srvs/srv/outputs_new.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <agv_srvs/srv/controller_enable.hpp>
#include <vector>
#include <controller_manager_msgs/srv/list_controllers.hpp>
#include <controller_manager_msgs/msg/controller_state.hpp>
#include <yaml-cpp/yaml.h>
#include <thread>
#include <mutex>
#include <fstream>

#include <agv_peripheral_controller/amr/pallet_controller.h>
#include <agv_peripheral_controller/peripheral_event.h>

#define JACKING_JOINT "jacking_joint"
#define ROTARY_JOINT "rotary_joint"
#define PROVELOBJ "6081"
#define OPERATE_TIMEOUT_S (10)
#define MOVE_TIMEOUT_S (60)
#define PACKAGE_NAME "agv_peripheral_controller"
#define FILE_NAME "/params/posRecord.yaml"


#define AGV_PERIPHERAL_CONTROLLER_NODE_ID   (2)

using std::placeholders::_1;
using std::placeholders::_2;

namespace agv_peripheral
{

  class PalletController;
  
  class AgvPeripheral:public rclcpp::Node
  {
  public:
    enum Type
    {
      JACKING,
      ROTARY,
      JACKING_ROTARY,
      ROLLER,
      FORKLIFT
    } peripheral_type;

    using PeripheralAction = agv_actions::action::Peripheral;
    using GoalHandle = rclcpp_action::ServerGoalHandle<PeripheralAction>;

    AgvPeripheral(const std::string &name = "agv_peripheral_controller");
    ~AgvPeripheral();

    bool init();
    void spin();
    void shutdown() { shutdown_req = true; };
    void handle_preemption();
    rclcpp_action::GoalResponse goalCB(const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const PeripheralAction::Goal> goal);
    rclcpp_action::CancelResponse cancelCB(const std::shared_ptr<GoalHandle> goal_handle);
    void acceptedCB(const std::shared_ptr<GoalHandle> goal_handle);
    void execute_action(const std::shared_ptr<GoalHandle> goal_handle);
    void periphStateMachineUpdate();
    void periphStatusGather();
    bool periphMotorErrCheck();
    bool periphMotorEnableCheck();
    bool periphHomingCheck();
    bool periphReadyCheck();
    bool periphGetControllerRunningStatus();

    PalletController* get_pallet_controller() { return pallet_controller;};
  protected:
    rclcpp_action::Server<PeripheralAction>::SharedPtr as;
    std::shared_ptr<rclcpp_action::ServerGoalHandle<PeripheralAction>> current_goal_handle_;
    std::shared_ptr<PeripheralAction::Result> current_result_;
    std::shared_ptr<PeripheralAction::Feedback> current_feedback_;
    std::mutex data_mutex_;
    
    void initialize_action_server() {
      as = rclcpp_action::create_server<PeripheralAction>(
          this,
          "do_peripheral",
          std::bind(&AgvPeripheral::goalCB, this, _1, _2),
          std::bind(&AgvPeripheral::cancelCB, this, _1),
          std::bind(&AgvPeripheral::acceptedCB, this, _1)
      );
    }

    void clear_current_action() {
        std::lock_guard<std::mutex> lock(data_mutex_);
        current_goal_handle_.reset();
        current_result_.reset();
        current_feedback_.reset();
    }

  private:
    std::string name;
    double frequency;
    bool shutdown_req;
    bool load_with_qrcode;
    bool find_joint_id;
    int joint_id[2];
    float max_pos[2];  //unit is mm or rads
    float min_pos[2];  //unit is mm or rads
    float max_pos_user[2]; //unit is mm or deg
    float min_pos_user[2]; //unit is mm or deg
    float rotary_target_pos;
    float previous_pos[2];
    float joint_pos[2],last_joint_record_pos[2];
    float file_record_pos[2];
    bool read_record_result;
    bool periphInitialized = false;
    // int jacking_speed;
    std::string pos_record_file;
    rclcpp::Time operate_time;
    rclcpp::Time jointState_update_time;
    bool followup_flag;
    int  bit_pos_rotate = 8;
    int  bit_forbid_rotary = 9;
    double rotary_manual_acc;
    double rotary_manual_vel;
    
    bool jacking_home_need;
    bool rotary_home_need;
    bool rotary_return_record_pos;

    bool hasliftrelay = false;
    
    std::string action_pick = "pick";
    std::string action_drop = "drop";
    std::string action_adjust = "lhdAdjust";

    bool jacking_move_state = false;
    bool rotary_move_state = false;
    bool jacking_arrive_recordPos = false;
    bool rotary_arrive_recordPos = false;
    rclcpp::Time jacking_move_time;
    rclcpp::Time rotary_move_time;

    uint16_t  last_jacking_status_word;
    uint16_t  last_rotary_status_word;

    rclcpp::Time err_begin_time;
    bool last_jacking_err;
    bool last_rotary_err;
    bool jacking_recover_emit;
    bool rotary_recover_emit;

    bool manual_jacking_active;
    bool rotary_manual_active_clock;
    bool rotary_manual_active_anticlock;
    rclcpp::Time rotary_manual_start_time_clock;
    rclcpp::Time rotary_manual_start_time_anticlock;
    rclcpp::Time rotary_manual_stop_time_clock;
    rclcpp::Time rotary_manual_stop_time_anticlock;
    bool rotary_manual_stop_flag_clock = false;
    bool rotary_manual_stop_flag_anticlock = false;
    double jacking_timeout_time;
    rclcpp::Time last_jacking_time;
    double rotary_timeout_time;
    rclcpp::Time last_rotary_time;
    rclcpp::Time last_landmark_shelf_time;
    rclcpp::Time last_event_time;
    rclcpp::Time last_set_stop_time;

    /*** global variables ***/
    sensor_msgs::msg::JointState joint_state; /**< joint state */
    std::mutex m_mutex;
    float manual_target_pos[2];      /**< manual ctl mode position */
    std_msgs::msg::Float64 cmd_pos; /**< float standard message corresponding to the command topic */
    std::vector<agv_msgs::msg::PeriphMotorInfo> periphMotor;
    agv_msgs::msg::PeriphStatus periphStatus;
    agv_msgs::msg::Load loadInfo;

    std::vector<agv_msgs::msg::Load> loads;
    agv_msgs::msg::DriversInfo driversInfo;
    agv_msgs::msg::SetStopBits setStop;

    // canopen_chain_node::SetObject setobject;
    
    agv_msgs::msg::Action current_action;

    std::thread m_thread;
    std::timed_mutex m_quit_mutex;

    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr js_sub;    /**< joint state from hardware interface */
    rclcpp::Client<agv_srvs::srv::ControllerEnable>::SharedPtr PeripheralEnableCtl; /**< joint point halt control */
    // rclcpp::Client<agv_srvs::srv::ControllerEnable>::SharedPtr peripheralGetcontrollers;
    rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr outputClient;
    rclcpp::Subscription<agv_msgs::msg::PeriphTeleop>::SharedPtr periph_teleop_sub;
    rclcpp::Publisher<agv_msgs::msg::PeriphStatus>::SharedPtr periph_status_pub;
    rclcpp::Subscription<agv_msgs::msg::AGVMode>::SharedPtr agvMode_sub; 
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr landmark_shelf_sub; 
    // rclcpp::Publisher<agv_msgs::msg::Event>::SharedPtr event_pub;
    rclcpp::Subscription<agv_msgs::msg::DriversInfo>::SharedPtr driver_info_sub;
    rclcpp::Publisher<agv_msgs::msg::SetStopBits>::SharedPtr set_stop_pub;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr auto_status_sub;
    rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr readinput_sub;

    PalletController* pallet_controller;
    PeripheralEvent pevent;

    // void reconfigCB(agv_peripheral_controller::periphDynParamConfig &config, uint32_t unused_level);
    void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &ms);
    bool executeOP(const int order);
    void periphTeleopCtlcb(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg);
    // void agvInfocb(const agv_msgs::msg::AgvInfo::ConstSharedPtr &msg);
    void agvModecb(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg);
    void landmarkShelfCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &data);
    void driversInfoCallback(const agv_msgs::msg::DriversInfo::ConstSharedPtr &msg);
    void autoStatusCallback(const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr &msg);
    void readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg);
    int callControllerService(const std::vector<std::string>& controllers, const bool enable);

    static void thread_fun(AgvPeripheral *pThis);
    void thread_cycle();
    void periphPosRecord();
    bool periphRecordPosRead();
    bool periphRecordPosWrite();
    bool periphMoveToRecordPos();
    void check_jointState_out();
    void set_stop_bits();
    void periphRecoverDetect();
    bool get_shared_parameters();

    enum periph_op_enum
    {
      NO_OP,
      UP,
      DOWN,
      CLOCKWISE_ROTATION,
      ANTICLOCKWISE_ROTATION,
      ROTATION,
      UP_CLOCKWISE_ROTATION,
      UP_ANTICLOCKWISE_ROTATION,
      CLOCKWISE_ROTATION_DOWN,
      ANTICLOCKWISE_ROTATION_DOWN,
    };

    enum periph_stata_enum
    {
      UNCONTROLLED_STATE = -1,
      IDLE_STATE,
      JACKING_OP_STATE_UP,
      JACKING_OP_STATE_DOWN,
      JACKING_SUCCESS_STATE,
      ROTARY_OP_STATE_CLOCKWISH,
      ROTARY_OP_STATE_ANTICLOCKWISH,
      ROTARY_OP_STATE_ROTATION,
      ROTARY_SUCCESS_STATE,
      MANUAL_MODE_STATE,
      HALT_HANDLE_STATE,
    };

    enum periph_result_enum
    {

      SUCCESS,
      FAILED,
      CANCELED,
      OTHER,
    };


    struct OperateState
    {
      int cur_state;
      int cur_op;
      int next_op;
    } os;

    struct peripheral_motor_status
    {
      bool enableState;
      bool homeState;
      bool motorErr;
      bool movingState;
      bool ready;
    }motorStatus[2];

    struct periph_input_status
    {
      bool manualState;
      bool handle_emergency_stop;
      bool emgButtonState;
      bool front_and_rear_bumper;
      bool upControl;
      bool downControl;
    }input_status;

    bool jacking_up_manual;
    bool jacking_down_manual;
    struct periph_read_input
    {
      bool reset;
      bool emergency_stop_1;
      bool emergency_stop_2;
    }read_input_data, last_read_input_data;

  public:
    void set_cur_op(periph_op_enum op);
    int get_cur_op();
    void set_next_op(periph_op_enum op);
    int get_next_op();
    void set_cur_state(periph_stata_enum op);
    int get_cur_state();
    void clear_os();
  };
} // agv_peripheral